/***************************************************************************
 创建者: 华磊
 开始时间: 2019.6.13
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************/
#include "velocityprofile_sin.h"
#include "velocityprofile_rect.hpp"
#include "velocityprofile_spline.hpp"
#include <QDebug>

#define IF_DEBUG_IMPROVED 0
#define D_NOMOVE_DISTANCE 0.001

namespace KDL {
VelocityProfile_sin::VelocityProfile_sin(double _maxvel, double _maxacc)
{
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"VelocityProfile_sin _maxvel"<<_maxvel<<"_maxacc"<<_maxacc;
    }

    maxvel=_maxvel;
    maxacc=_maxacc;
    isOneVelProfile=false;
    isNoMoveFlag=false;
}

VelocityProfile_sin::~VelocityProfile_sin()
{

}
#define D_SPLINE_TIME_RATIO 2
int VelocityProfile_sin::SetProfile_transsition(double pos1, double vel1, double pos2, double &vel2,bool isStartChange)
{



}


void VelocityProfile_sin::SetProfile(double pos1, double pos2)
{
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"VelocityProfile_sin::SetProfile pos1"<<pos1<<"pos2"<<pos2<<"maxvel"<<maxvel<<"maxacc"<<maxacc;

    }
    if(fabs(pos1-pos2)<D_NOMOVE_DISTANCE)
    {
        isNoMoveFlag=true;
        duration=0;
        startpos=pos1;
        endpos=pos2;
        isOneVelProfile=true;
        c1=0;
        t_increaseFinish=0;
        t_decreaseStart=0;
        realMaxVel=0;
        s_increaseFinish=startpos;
        s_decreaseStart=startpos;
        return;
    }

    startpos=pos1;
    endpos=pos2;

    double lineLength=fabs(pos1-pos2);
    k1=maxacc;
    //根据最大加速度和距离，求速度，看速度是否超过最大加速度
    double t_accUp=sqrt(lineLength*M_PI/k1/2.0);
    double tmpVel=2*k1/(M_PI/t_accUp);
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"VelocityProfile_sin t_accUp"<<t_accUp<<"lineLength"<<lineLength<<"maxacc"<<maxacc<<"tmpVel"<<tmpVel;
    }

    //如果总长小于一定值，则直接用一段规划，因为可能距离太短，达不到匀速段。
    if(tmpVel<maxvel)
    {
        isOneVelProfile=true;
        c1=2*k1/tmpVel;
        t_increaseFinish=M_PI/c1;
        t_decreaseStart=t_increaseFinish;
        realMaxVel=tmpVel;
        if(pos2>pos1)
        {
            s_increaseFinish=lineLength/2.0+startpos;
        }
        else
        {
            s_increaseFinish=-lineLength/2.0+startpos;
        }

        s_decreaseStart=s_increaseFinish;

    }
    else
    {
        isOneVelProfile=false;
        c1=2*k1/maxvel;
        t_increaseFinish=M_PI/c1;
        double tmpS=k1*t_increaseFinish/c1;
        t_decreaseStart=t_increaseFinish+(lineLength-2*tmpS)/maxvel;
        realMaxVel=maxvel;
        if(pos2>pos1)
        {
            s_increaseFinish=k1*t_increaseFinish/c1+startpos;
            s_decreaseStart=pos2-k1*t_increaseFinish/c1;
        }
        else
        {
            s_increaseFinish=-k1*t_increaseFinish/c1+startpos;
            s_decreaseStart=pos2+k1*t_increaseFinish/c1;
        }

    }


    duration=t_decreaseStart+t_increaseFinish;
    timeRatio=1.0;
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"duration"<<duration<<"isOneVelProfile"<<isOneVelProfile<<"t_increaseFinish"<<t_increaseFinish
                  <<"t_decreaseStart"<<t_decreaseStart;

    }
}

void VelocityProfile_sin::SetProfile_ZeroDuration(double pos1, double pos2, double newduration)
{
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"VelocityProfile_sin::SetProfile_ZeroDuration pos1"<<pos1<<"pos2"<<pos2
               <<"maxvel"<<maxvel<<"maxacc"<<maxacc<<"newduration"<<newduration;

    }

    SetProfile(pos1,pos2);
    if(pos1==pos2)
    {
        timeRatio=1;
        return;
    }
    timeRatio=duration/newduration;
    if(IF_DEBUG_IMPROVED)
    {
        qDebug()<<"duration"<<duration<<"isOneVelProfile"<<isOneVelProfile<<"t_increaseFinish"<<t_increaseFinish
                  <<"t_decreaseStart"<<t_decreaseStart<<"timeRatio"<<timeRatio;

    }
}

void VelocityProfile_sin::SetProfile_tozero(double pos1, double vel1, double pos2)
{

}



void VelocityProfile_sin::SetProfileDuration(double pos1, double pos2, double newduration)
{


}

void VelocityProfile_sin::SetProfileVelocity(double pos1, double vel1, double pos2, double vel2, double newvelocity)
{

}

void VelocityProfile_sin::SetMax(double _maxvel, double _maxacc)
{
    maxvel=_maxvel;
    maxacc=_maxacc;

}

double VelocityProfile_sin::Duration() const
{
    if(isNoMoveFlag)
    {
        return 0;
    }
    return duration/timeRatio;
}

double VelocityProfile_sin::Pos(double time) const
{
    if(isNoMoveFlag)
    {
        qDebug()<<"warn:VelocityProfile_sin noMoveFlag true";
        return endpos;
    }

    time=time*timeRatio;
    double tmpPose;

    //使用公式计算，拼接过渡点由于精度关系，会导致位置变换量不连续．
    if(time<=t_increaseFinish)
    {
        if(endpos>startpos)
        {
            tmpPose= startpos+k1*(time/c1-sin(c1*time)/c1/c1);
        }
        else
        {
            tmpPose= startpos-k1*(time/c1-sin(c1*time)/c1/c1);
        }

    }
    else
    {
        if(isOneVelProfile)
        {
            time=duration-time;
            if(endpos>startpos)
            {
                tmpPose=  endpos-k1*(time/c1-sin(c1*time)/c1/c1);
            }
            else
            {
                tmpPose=  endpos+k1*(time/c1-sin(c1*time)/c1/c1);
            }

        }
        else
        {
            if(time<=t_decreaseStart)
            {
                if(endpos>startpos)
                {
                    tmpPose= s_increaseFinish+maxvel*(time-t_increaseFinish);
                }
                else
                {
                    tmpPose= s_increaseFinish-maxvel*(time-t_increaseFinish);
                }

            }
            else
            {
               time=duration-time;
               if(endpos>startpos)
               {
                   tmpPose=  endpos-k1*(time/c1-sin(c1*time)/c1/c1);
               }
               else
               {
                   tmpPose=  endpos+k1*(time/c1-sin(c1*time)/c1/c1);
               }
            }
        }
    }

    if(IF_DEBUG_IMPROVED)
    {
    qDebug()<<"tmpPose"<<tmpPose;
    }
    return tmpPose;

}

double VelocityProfile_sin::Vel(double time) const
{
    if(isNoMoveFlag)
    {
        return 0;
    }
    time=time*timeRatio;
    double tmpVel;

    if(time<=t_increaseFinish)
    {
        tmpVel= k1*(1-cos(c1*time))/c1;
    }
    else
    {
        if(isOneVelProfile)
        {
            tmpVel=  realMaxVel-k1*(1-cos(c1*(time-t_increaseFinish)))/c1;
        }
        else
        {
            if(time<=t_decreaseStart)
            {
                tmpVel= realMaxVel;
            }
            else
            {
               tmpVel=  realMaxVel-k1*(1-cos(c1*(time-t_decreaseStart)))/c1;
            }
        }
    }
    if(IF_DEBUG_IMPROVED)
    {
    qDebug()<<"tmpVel"<<tmpVel;
    }
    return tmpVel;
}

double VelocityProfile_sin::Acc(double time) const
{
    if(isNoMoveFlag)
    {
        return 0;
    }
    time=time*timeRatio;
    double tmpacc;
    if(time<=t_increaseFinish)
    {
        tmpacc= k1*sin(c1*time);
    }
    else
    {
        if(isOneVelProfile)
        {
            tmpacc=  -k1*sin(c1*(time-t_increaseFinish));
        }
        else
        {
            if(time<=t_decreaseStart)
            {
                tmpacc= 0;
            }
            else
            {
               tmpacc=  -k1*sin(c1*(time-t_decreaseStart));
            }
        }
    }

    if(IF_DEBUG_IMPROVED)
    {
    qDebug()<<"tmpacc"<<tmpacc;
    }
    return tmpacc;
}

void VelocityProfile_sin::Write(std::ostream &os) const
{

}

VelocityProfile* VelocityProfile_sin::Clone() const
{
//    VelocityProfile_sin* tmpVel=new VelocityProfile_sin(maxvel,maxacc);
////    tmpVel->SetProfile(startpos,startVel,endpos,endVel);
//    return tmpVel;
//    return new VelocityProfile_sin();
}
}
